#pragma once

#include <iostream>
#include <string>
#include <exception>
#include <cstdlib>
#include <unistd.h>
#include <serial/serial.h>
#include <rclcpp/rclcpp.hpp>
#include <vector>
#include <cstdint>

#include "robot_msgs/msg/hardware.hpp"
#include "robot_msgs/msg/led_state.hpp"
#include "robot_msgs/msg/sonar_state.hpp"


// ===================== 宏定义 =====================
#define FRAME_HEADER_1      0x55    // 1 字节帧头
#define FRAME_HEADER_2      0xAA    // 1 字节帧头
#define FRAME_FOOTER        0xAA    // 帧尾

#define FRAME_MIN_LEN       6       // 最小帧长检查（帧头(2)+Length(1)+CmdID(1)+CRC(1)+帧尾(1)=6）
#define FRAME_HEADER_SIZE   2       // 帧头 
#define FRAME_LENGTH_SIZE   1       // length 大小
#define FRAME_CMDID_SIZE    1       // CmdID 大小
#define FRAME_CRC_SIZE      1       // CRC 大小
#define FRAME_FOOTER_SIZE   1       // 帧尾大小

#define BUFFER_MAX_SIZE     1024    // 缓冲区最大长度
#define BUFFER_SAFE_SIZE    512     // 安全缓冲区大小

#define CMD_ENCODER         0x01    // 速度控制命令
#define CMD_BLADE           0x02    // 刀盘控制命令
#define CMD_STATE           0x03    // 状态请求命令
#define CMD_STATE_BLADE     0x70    // 刀盘状态
#define CMD_STATE_BATTERY   0x71    // 电池状态

// ===================== 结构体 =====================
struct EncoderCounts {
    int32_t left_mm = 0;
    int32_t right_mm = 0;
    std::chrono::steady_clock::time_point stamp_system;  // 系统时间
    rclcpp::Time stamp_ros;  // ROS 时间（由上层补）

    EncoderCounts deltaFrom(const EncoderCounts &last) const {
        EncoderCounts delta;
        delta.left_mm = left_mm - last.left_mm;
        delta.right_mm = right_mm - last.right_mm;
        delta.stamp_ros = stamp_ros;
        return delta;
    }

    double deltaTimeSec(const EncoderCounts &last) const {
        return (stamp_ros - last.stamp_ros).seconds();
    }
};

struct HardwareStates {
    // 实时系统状态
    uint8_t bat_level = 0;          // 当前电池电量（0~100）
    float bat_voltage = 0.0f;       // 当前电池电压（单位 V）
    float bat_temperature = 0.0f;   // 电池温度（单位 ℃）
    float total_current = 0.0f;     // 整机工作电流（单位 A，充电为负）
    float core_temperature = 0.0f;  // 整机核心温度（单位 ℃）
    float sys_temperature = 0.0f;   // 系统温度（单位 ℃）

    // 工作状态
    int8_t cutter_spd_state = 0;    // 割草盘转速（128为最大顺转，0停止，-127为最大反转）
    uint8_t charge_state = 0;       // 充电状态：0放电，1充电，2充电故障
    float left_wheel_spd = 0.0f;    // 左轮实时速度（单位 m/s）
    float right_wheel_spd = 0.0f;   // 右轮实时速度（单位 m/s）
    robot_msgs::msg::LedState led_state; // 指示灯状态（自定义编码）

    // 传感器状态
    robot_msgs::msg::SonarState sonar_state; // 超声波使能状态（1为使能，0为禁用）超声波距离数据（单位 m）


    // 版本状态
    std::string software_version; // 软件版本字符串
    std::string hardware_version; // 硬件版本字符串
    std::string serial_id;        // 产品唯一序列号

    // 更新时间戳
    std::chrono::steady_clock::time_point stamp_system;  // 系统时间
    rclcpp::Time stamp_ros;  // ROS 时间（由上层补）
};


// ===================== 串口硬件封装类 =====================
class HardwareInterface {
    public:
        HardwareInterface();
        virtual ~HardwareInterface();

        // 初始化串口通信
        bool SerialInit(const std::string & port, int baudrate);

        // 获取串口数据
        bool GetSerialData(void);

        // 解析数据帧
        bool ParseRecvBuffer(const std::vector<uint8_t>& valid_frame);

        // 解析编码器数据
        void HandleEncoder(const std::vector<uint8_t>& data);

        void HandleBlade(const std::vector<uint8_t>& data);

        // 发送速度控制命令
        void SendVelocityCommand(double left_mps, double right_mps);

        // 发送刀盘控制命令
        void SendCutterSpdCtlCmd(int8_t cutter_spd);

        // 
        void SendGetCutterSpdCmd(void);

        //
        void SendGetBatStateCmd(void);

        // 发送串口原始数据
        bool WriteData(const std::vector<uint8_t>& data);

        // 附加16位整型（大端）到缓冲区
        inline void AppendInt16BigEndian(std::vector<uint8_t>& buf, int16_t val); 

        // 从大端字节解析32位整型
        int32_t ParseInt32BE(const std::vector<uint8_t>& data, size_t offset);

        // CRC校验
        uint8_t CalculateCRC(std::vector<uint8_t>::const_iterator start, std::vector<uint8_t>::const_iterator end);
        bool CheckCRC(const std::vector<uint8_t>& frame, uint8_t crc);

        // 判断串口是否正常
        bool IsSerialOK() const;

        // 获取编码器数据
        EncoderCounts GetMotorEncoderCounts() const;

        // 
        HardwareStates GetHardwareStates() const;

    protected:
        serial::Serial __stm32_serial;                 // 串口对象
        std::vector<uint8_t> __recv_buffer;            // 接收缓冲区
        EncoderCounts __motor_encoder_counts;          // 编码器累计计数（单位：mm）
        HardwareStates __hardware_states;
};

